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Abstract 

We  consider  the  problem  of  planning  and  predicting  the  motion  of  a  flexible 
object  ^amidst  obstacles  in  the  plane^-Ws  -model  the  flexible  object  as  a  rigid 
“root”  body,  attached  to  compliant  members  by  torsional  springs.  The  root’s 
position  may  be  controlled,  but  the  compliant  members  move  in  response  to 
forces  from  contact  with  the  environment.  Such  a  model  encompasses  several 
important  and  complicated  mechanisms  in  mechanical  design  and  automated 
assembly:  snap-fasteners,  latches,  ratchet  and  pawl  mechanisms,  and  escape¬ 
ments.  The  problem  is  to  predict  the  motion  of  such  a  mechanism  amidst  fixed 
obstacles.  For  example,  our  algorithm  could  be  used  to  determine  whether  a 
snap-fastener  design  can  be  assembled  with  a  certain  plan. 

la  this  paper  uflfanalyz^the  physics  of  these  flexible  devices,  and  develop  the 
first  combinatorially  precise  algorithm  for  predicting  their  movement  under  a 
motion  plan.  Quf^algorithm  determines  when  and  where  the  motion  will  termi¬ 
nate,  and  also  computes  the  time-history  of  contacts.  In  addition  to  providing 
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the  first  known  exact  algorithm  that  addresses  flexibility  in  motion  planning,  we 
also  note'that  our  approach  to  compliance  permits  an  exact  algorithm  for  pre¬ 
dicting  motions  under  rotational  compliance,  which  was  not  possible  in  earlier 
work. 

1  Introduction 

Previous  work  on  algorithmic  motion  planning  has  largely  concentrated  on  the  move¬ 
ment  problem  for  rigid  bodies  [LP83,B+82,Yap87].  Recently,  work  in  compliant  mo¬ 
tion  planning  under  uncertainty  [LMT84,Erd84,Don88b]  has  focussed  on  the  prob¬ 
lem  of  moving  rigid  objects  (e.g.  pegs)  in  contact  (i.e.,  compliantly)  with  obstacles 
(such  as  holes)  under  force-control,  subject  to  bounded  uncertainty  and  error.  How¬ 
ever,  real  objects  that  robots  might  assemble  axe  typically  not  rigid.  For  example, 
a  Sony  Walkman  is  made  of  plastic  parts  that  snap  together.  Significant  advances 
were  made  in  the  design  of  the  IBM  ProPrinter,  by  replacing  traditional  fasteners 
such  as  screws  with  plastic  parts  that  simply  snap  together.  The  reason  these  plas¬ 
tic  parts  snap  together  is  that  they  are  flexible:  more  precisely,  they  are  passively 
compliant  This  means  that  when  the  parts  are  brought  together  and  an  external 
force  applied,  the  parts  deform  in  a  prescribed  way.  More  interestingly,  the  force 
required  to  mate  two  parts  may  be  much  less  than  the  force  required  to  take  them 
apart. 

Since  we  wish  to  be  able  to  design  and  have  our  robots  assemble  such  objects 
given  task-level  descriptions,  we  must  have  a  systematic  program  for  reasoning  about 
and  predicting  their  motions  in  contact.  To  this  end,  we  will  first  make  precise  a 
sufficiently  powerful  notion  of  flexibility  to  model  the  objects  above.  Our  model  is 
based  on  the  well  known  generalized  spring  in  the  robotics  and  controls  literature 
(eg.,  [Mas8l]).  We  will  model  the  physics  of  interaction  of  the  flexible  parts  and  the 
obstacles  using  quasi-static  analysis  and  Coulomb  friction.  Then,  using  these  tools, 
we  will  proceed  to  develop  a  combinatorially  precise  algorithm  for  predicting  the 
motion  of  a  flexible  object  amidst  obstacles.  The  algorithm  is  roughly  0(N 2  log  N) 
in  the  geometric  complexity,  N.  A  system  based  on  the  results  of  this  paper  is 
described  in  the  companion  paper  [DP89],  which  also  discusses  its  relevance  to  the 
design  of  such  mechanisms. 

From  a  complexity-theoretic  standpoint,  our  result  may  also  be  viewed  as  follows. 
Earlier  work  on  compliant  motion  planning  [LMT84,Erd84,Don88bj  has  employed 
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Figure  1:  Linked  body  M  moving  among  Af. 


a  dynamic  model  called  the  generalized,  damper  [Whi76].  While  this  model  has 
led  to  exact  algorithms  in  the  case  of  pure  translations  [Don88b,Can89],  so  far  we 
have  not  been  able  to  provide  combinatorially  precise  algorithms  once  rotations 
are  permitted.  This  is  true  even  if  the  commanded  motion  is  a  pure  translation 
and  the  moving  object  (eg.,  the  peg)  is  merely  rotationally  compliant.  One  reason 
for  this  difficulty  seems  to  be  that  under  the  damper  model,  the  resulting  motions 
are  not  obviously  algebraic;  even  more  disturbing,  the  outer  envelope  (or  forward 
projection )  of  these  motions  cannot  yet  be  algebraically  described.  (This  is  also  a 
problem  in  many  more  complicated  dynamical  systems).  However,  under  the  model 
in  this  paper,  rotationally  compliant  subparts  are  permitted,  and  yet  all  resultant 
trajectories  of  the  system  are  algebraic.  Finally,  we  believe  that  our  algorithm  is 
sufficiently  simple  that  it  could  be  implemented,  and  used  to  plan  and  verify  the 
design  of  flexible  mechanisms  for  ease  of  assembly. 


1.1  Problem  Statement 

We  consider  the  problem  of  moving  a  flexible,  linked  body  M  in  the  plane,  in  a 
polygonal  environment  A/.  The  flexible  body  M  is  constructed  as  follows:  polygons 
Mh,  h  =  1, . . . ,  /,  called  “pawls",  are  attached  to  a  root  polygon  Mo,  at  hinge  points 
Ph-  Each  hinge  is  coupled  with  a  spring  of  stiffness  1*.  (See  Figure  1).  The  motion 
of  the  body  M  consists  of  rigid  translation  of  the  root  polygon  Mo-  The  pawls  Mu 
are  free  to  move  compliantly  as  dictated  by  interactions  with  the  environment  and 
the  spring. 

The  Motion  Prediction  Problem  is  to  determine:  _ _ 


□ 
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1.  The  time  of  termination  of  the  motion,  the  cause  of  termination  (such  as 
sticking  due  to  friction,  sticking  due  to  kinematic  constraints,  etc.),  and  the 
configuration  of  M  at  termination. 

2.  The  time  history  of  contacts  between  M  and  Af. 

Some  extensions  of  the  Motion  Prediction  Problem  are  considered  in  our  companion, 
paper  [DP89],  including  the  determination  of  the  time  history  of  forces  during  the 
motion,  and  the  effect  of  uncertainty. 

We  make  the  following  assumptions  about  the  physics  of  object  interactions  and 
the  motion  of  M: 

•  Object  interactions  are  restricted  to  those  between  Mh  and  M .  In  other  words, 
pawls  do  not  collide  with  each  other,  but  may  collide  with  the  environment  Af . 
The  effect  of  this  assumption  is  to  make  the  motion  of  each  pawl  independent 
of  the  motion  of  other  pawls.  Henceforth  we  shall  consider  the  motion  of  a 
single  pawl  Mh- 

•  Since  the  root  M0  is  undergoing  a  rigid  translation,  so  does  the  hinge  point 
Ph.  We  shall  assume  that  this  translation  is  a  straight  line  motion  given  by 
p  =  Po+pt  where  t  is  the  time,  p0  is  the  initial  position  (at  t  =  0)  of  Ph,  and 
p  is  its  velocity. 

•  Stable  contact:  Suppose  the  pawl  is  in  contact  with  a  feature  of  the  environ¬ 
ment  (for  example,  during  sliding).  We  assume  that  if  we  perform  a  small 
displacement  of  the  pawl  away  from  the  environment,  the  torque  on  the  pawl 
due  to  the  spring  is  such  that  the  contact  will  be  restored.  This  assumption  is 
not  very  restrictive  at  all  -  in  fact,  in  the  face  of  even  the  smallest  uncertainty, 
stable  contacts  are  the  only  ones  one  can  hope  to  observe  in  practice. 

•  Quasi-static  motion:  The  motion  is  assumed  to  be  slow  enough  that  inertial 
effects  are  not  significant.  This  corresponds  to  assuming  that  there  is  no 
acceleration  of  the  pawl,  and  hence  the  forces  on  the  pawl  are  balanced.  The 
quasi-static  assumption  is  reasonable  at  small  speeds  and  is  widely  used  (see, 
for  example,  (Whi82,Mas82,Don88a,Pai88j). 
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•  If  the  pawl  slides  off  Af  into  free  space,  it  may  have  a  residual  torque  due  to  the 
jpring  being  cocked.  We  shall  assume  that  the  pawl  rotates  back  towards  its 
rest  orientation  at  such  great  speed  that  p  does  not  change  significantly  during 
the  rotation  and  can  be  taken  to  be  constant.  This,  incidentally,  is  the  “snap” 
in  the  snap-fasteners  that  we  wish  to  model.  This  assumption  may  appear 
to  contradict  the  assumption  of  quasi-static  motion,  but  is  in  fact  practically 
a  consequence.  Quasi-static  motion  implies  that  the  root  is  moving  “slow- 
enough”  for  the  forces  to  be  balanced.  Hence,  when  a  pawl  is  in  free  space 
and  has  a  residual  torque,  its  motion  can  be  fast  compared  to  that  of  the  root, 
resulting  in  a  “snap”.  This  assumption  can  be  relaxed  by  assuming  a  linear 
relationship  between  the  translation  p  and  the  rotation  (e.g.,  see  [Can86j),  but 
we  do  not  deal  with  it  here. 

•  The  forces  of  friction  arising  from  contact  obey  Coulomb’s  Law.  We  further 
assume  that  there  is  a  single  coefficient  of  friction.  This  assumption  is  also 
widely  used. 

Two  types  of  contact  are  possible  between  the  pawl  Mh  and  a  polygon  in  Af. 
Following  the  convention  of  Lozano- Perez  [LP83]  and  Donald  [Don87],  we  say  that 
Type- A  contact  occurs  when  a  vertex  of  Af  touches  an  edge  of  the  pawl;  Type-B 
contact  occurs  when  a  vertex  of  the  pawl  touches  an  edge  of  Af. 

We  can  now  write  the  contact  constraint  equations  for  the  two  types  of  contact, 
as  in  [Can86].  We  shall  index  features  (vertices  and  edges)  of  the  moving  pawl  by 
the  subscript  i  and  features  of  the  polygonal  environment  by  the  subscript  j .  Let  an 
edge  of  Mh  be  represented  by  its  outward  normal,  r*,-,  and  its  distance  to  the  hinge 
point  along  the  normal,  d,.  Let  pj  be  a  vector  to  the  contact  vertex  of  Af.  Let  R$ 
be  the  linear  transformation  which  rotates  a  vector  by  an  angle  9.  Then  the  type-A 
constraint  can  be  written  as 


(Pi  -di  =  0.  (1) 

Similarly,  the  type-B  constraint  can  be  written  as 

(Rtpi  +  p)-nj  -dj  =  0.  (2) 


Where  pi  is  the  vector  from  the  hinge  point  Pk  to  the  contact  vertex  oi  Mh- 


Figure  2:  Geometric  Interpretation  of  Coulomb’s  Law 

2  Sticking  due  to  Friction 

A  careful  analysis  of  the  physics  of  compliant  sliding  is  necessary  to  formulate  a  pre¬ 
cise,  combinatorial  version  of  this  apparently  continuous  problem.  When  the  pawl 
is  in  comact  with  the  environment,  it  is  possible  for  the  motion  to  stop  because  the 
contact  forces  are  adequate  to  balance  the  applied  forces.  This  is  called  “sticking 
due  to  friction,”  to  distinguish  it  from  other  kinds  of  sticking  (see  Section  5).  The 
algorithm  must  determine  if  this  can  happen  during  motion.  The  works  of  Ma¬ 
son  [Mas82]  and  Erdmann  [Erd84]  address  this  issue  in  considerable  detail.  In  this 
section,  we  show  that  for  our  problem  this  determination  can  be  made  by  a  simple 
geometric  test. 

Coulomb’s  Law  of  friction  states  that  the  tangential  force  due  to  friction,  ft,  on  a 
point  which  is  sliding  on  an  edge  in  a  specified  direction  is  given  by  ft  =  /*/„«  where 
fn  is  the  force  normal  to  the  edge  and  n  is  the  coefficient  of  friction.  In  addition, 
the  direction  of  the  friction  force  is  opposite  that  of  the  motion. 

Coulomb’s  Law  has  a  geometric  interpretation  which  is  frequently  more  useful 
(see,  for  example,  [Mas82]).  We  provide  here  a  slightly  different  interpretation, 
which  is  more  applicable  to  our  problem.  Suppose  point  p,  is  in  contact  with  edge 
tj.  The  outward  pointing  unit  normal  vector  to  the  edge  is  denoted  «>.  We  are 
also  given  the  direction  of  sliding  by  specifying  v,  the  unit  vector  tangent  to  the 
edge  in  the  direction  of  sliding  (see  Figure  2).  Coulomb’s  Law  defines  a  half-cone  of 
forces  acting  on  the  point  p,.  The  cone  is  the  convex  combination  1  of  the  vectors 


‘The  convex  combination  of  two  vectors  *  and  y  is  the  set  of  ail  vectors  of  the  form  C*  +  W 
where  C,  H  >  0. 
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rij  and  the  vector  fe  =  n_,  —  pv.  Any  force  in  the  interior  of  this  cone  and  along  n: 
can  be  resisted -and  the  point  will  stick.  A  force  along  fe  will  result  in  p<  sliding  in 
equilibrium,  in  the  direction  of  v.  A  force  outside  the  cone  results  in  the  point  sliding 
along  v,  out  of  equilibrium,  which  is  precluded  by  our  assumption  of  quasi-static 
motion. 

We  first  consider  the  type-B  contact:  the  point  is  the  vertex  :  of  the  pawl,  and  the 
edge  is  the  edge  e3  of  the  environment.  Let  r  =  R$pi .  The  assumption  that  stable 
contact  is  maintained  (see  Section  1.1)  between  the  point  and  the  edge  restricts  the 
possible  values  of  the  force  f  on  the  pawl  and  the  torque  r  x  /  2  due  to  the  contact 
force.  First,  f-rij  must  be  positive;  second,  if  the  hinge  is  in  the  right  half-plane, 
we  require  r  x  /  <  0  for  stability,  and  if  it  is  in  the  left  half-plane,  we  require  that 
r  x  /  >  0.  For  quasi-static  sliding,  the  total  force  is  given  by  /  =  a fe,  a  >  0. 
Therefore  /  trivially  satisfies  the  condition  f-rij  >  0.  The  condition  on  the  torque 
r  x  /,  however,  depends  on  the  location  of  the  hinge  point.  Let  the  possible  locations 
of  the  hinge  point  with  respect  to  the  contact  point,  —  r,  be  divided  into  four  sectors 
as  follows:  if  — r  =  flrij  +  7 fe,  the  hinge  point  is  in  Sector 

I  if  /?  >  0,  7  >  0, 

II  if  0  <  0,  7  >  0, 

III  if  j3  <  0,  7  <  0, 

IV  if  /3  >  0,  7  <  0. 

Figure  2  also  indicates  the  sectors. 

Proposition  1.  For  type-B  contact,  the  contact  point  will  slide  when  the  hinge  is 
in  sectors  II  and  IV.  The  contact  point  will  not  slide  (will  stick  due  to  friction)  when 
the  hinge  is  in  sectors  I  and  III. 

Proof.  Suppose  the  pawl  is  sliding  quasi-statically.  Then, 

/  =  a/e 

=  a(n>  -  pv). 
r  =  -[/?n,  +  7/e] 

=  -  [(0  +  l)n,  ~  • 

JHere  r  x  f  is  the  component  of  the  usual  cross  product  along  the  axis  orthogonal  to  the  plane, 
i.e.,  in  coordinates,  r  x  f  =  r,/f  -  rt/,. 
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Figure  3:  Type- A  Contact 


r  x  /  =  -a  [0  -  (0  +  7)/i  +  7/i  +  0] 

=  c*0p.  (3) 

Since  a,  p  >  0,  the  sign  of  r  x  /  is  just  that  of  0.  Now,  in  sectors  I  and  II,  the 
right-half  plane,  we  require  that  r  x  /  <  0  to  maintain  stable  contact.  But  from 
Equation  3,  r  x  /  >  0  in  sector  I  and  r  x  /  <  0  in  sector  II.  Hence  the  contact 
point  will  stick  in  sector  I  and  slide  in  sector  II.  A  similar  argument  shows  that  the 
contact  point  will  stick  in  III  and  slide  in  IV.  □ 

A  similar  argument  applies  to  type- A  contact  if  the  sectors  and  related  terms 
are  defined  appropriately  (see  Figure  3).  Let  /  be,  as  before,  the  force  on  the  point 
of  contact,  which  is  now  a  vertex  j  of  the  environment,  and  let  r  be  the  vector  from 
the  hinge  point  to  the  contact  point,  which  is  now  given  by  r  =  pz  —  p.  Let  the 
outer  normal  of  the  edge  e,-  be  denoted  by  =  f2*rc,,  v  be  the  unit  vector  tangent 
to  the  edge  in  the  direction  in  which  the  vertex  slides  with  respect  to  the  pawl’s 
edge  and  /,  =  n»j  -  pv.  To  maintain  contact,  /-m,  must  be  positive  and,  if  the 
hinge  is  in  the  right  half-plane,  we  require  r  x  — /  >  0,  i.e.r  x  /  <  0,  and  if  it  is  in 
the  left  half-plane,  we  require  that  r  x  /  >  0. 

With  the  terms  defined  as  above,  we  can  divide  the  possible  locations  of  the  hinge 
point  with  respect  to  the  contact  geometry  into  four  sectors  exactly  as  we  did  for 
Type-B  contact,  despite  the  fact  that  the  interpretation  of  the  sectors  is  different. 
Now  it  is  easy  to  prove  the  corresponding  proposition: 


Proposition  2.  For  type-A  contact,  the  contact  point  will  slide  when  the  hinge  is 
in  sectors  II  and  TV.  The  contact  point  will  not  slide  (will  stick  due  to  friction)  when 
the  hinge  is  in  sectors  I  and  III. 
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Proof.  The  proof  is  textually  identical  to  the  proof  for  Type-B  contact,  with  n: 
replaced  by  m,'.  □ 

3  The  Sliding  Direction 


The  analysis  of  Section  2  utilized  the  direction  of  motion  to  determine  whether  the 
pawl  could  stick  due  to  friction.  It  turns  out  that  the  direction  of  the  motion  can 
be  determined  from  the  differential  kinematics  of  the  contact,  given  the  motion  of 
the  hinge,  p. 

Consider  the  motion  for  a  type-B  contact.  For  convenience,  assume  that  the  Y- 
axis  of  the  reference  coordinate  frame  is  along  the  outward  normal  of  the  edge,  i.e., 
y  =  rij,  and  the  X-axis  is  along  the  edge,  oriented  to  form  a  right-handed  coordinate 
system.  Recall  that  r  =  RePi.  Then,  differentiating  the  type-B  kinematic  constraint 
equation,  Equation  2,  we  get 

(*■  +  P)'nj  ~  0.  (4) 

Now  f  =  w(— ry,  rx)T,  where  u  is  the  angular  velocity  of  the  pawl.  Hence,  the  above 
equation  can  be  written  as 

wr*+p'w  =  0.  (5) 

This  equation  can  be  derived  in  another  way,  which  will  be  useful  in  Section  5. 
Define  ihc  departure  .speed,  u,  as  the  component  of  the  velocity  of  the  contact  point 
along  the  edge  normal  n,.  The  velocity  of  the  contact  point  is  given  by  f  +p.  Hence 

«s(f  +  p)rtj  =  urs  +  py.  (6) 

The  differential  kinematic  constraint  for  sliding  corresponds  to  the  requirement  that 
the  departure  speed  is  zero.  It  is  easy  to  see  that  the  two  derivations  are  equivalent. 

The  sliding  speed,  s ,  is  defined  as  the  component  of  the  velocity  of  the  contact 
point  in  the  x  direction  (the  vector  v  of  Section  2  is  therefore  sgn(s)x).  Hence, 


Combining  with  Equation  5, 


s  =  (f  +  p)-x 
=  -wr,  +  p*. 


Pv 

3  as  p.  +  — ru. 

r. 


(7) 

(8) 

(9) 


Equation  9  has  a  simple  geometric  interpretation,  shown  in  Figure  4.  The  circle 


Figure  4:  The  sliding  direction  for  Type-B  contact. 


represents  the  possible  positions  of  the  hinge  point  with  respect  to  the  contact  point. 
The  numbers  on  the  circle  indicate  the  sliding  speed,  when  py  <  0  (if  p„  >  0,  the 
speeds  shown  have  opposite  sign;  if  pv  -  0,  s  is  constant).  Notice  that  the  sliding 
speed  is  0  when  rp  =  0.  Also,  from  Equation  5,  u  <  0  when  the  hinge  point  (  — r) 
is  in  the  right  half-plane  of  the  figure,  and  u;  >  0  in  the  left  half-plane. 

We  can  now  combine  the  above  results  with  those  of  section  2.  Suppose,  without 
loss  of  generality,  that  the  hinge  point  is  in  the  right  half-plane.  If  the  hinge  is  in 
the  region  corresponding  to  s  <  0,  the  possible  direction  of  slide  is  to  the  left.  If,  in 
addition,  the  hinge  is  in  sector  I,  it  will  stick.  If  it  is  outside  sector  I,  the  location  of 
the  hinge  will  move  in  the  direction  of  u,  at  some  point  (corresponding  to  r  p  =  0), 
the  direction  of  slide  will  reverse  to  s  >  0.  This  also  brings  sectors  III  and  IV  to 
the  right  half-plane.  Eventually,  the  hinge  will  enter  sector  III  for  s  >  0,  and  stick. 
Figure  4  also  depicts  the  evolution  of  the  hinge  point. 

The  time  at  which  the  contact  will  stick  in  sector  HI  is  easily  computed  by  letting 
-j-  pv)  in  the  constraint  equation  2  and  solving  the  resulting  linear 

equation  for  t. 

A  similar  but  slightly  more  complicated  analysis  applies  to  Type-A  contact.  We 
shall  first  derive  an  expression  for  the  velocity  t o  of  the  contact  point  with  respect 
to  a  frame  fixed  on  the  moving  pawl,  and  expressed  in  the  world  coordinate  frame. 
We  shall  assume  for  simplicity  of  derivation,  that  the  coordinate  frame  attached 
to  the  pawl  has  its  Y-axis  along  the  edge  normal  m*  =  A#n,,  and  the  X-axis  is 
along  the  edge  so  as  to  form  a  right-handed  coordinate  system.  As  in  Section  2,  for 
Type-A  contact,  r  =  p}  -  p ,  and  let  pr  =  R«Tr  be  the  corresponding  vector  in  the 


coordinate  frame  fixed  to  the  pawl.  Let  Cl  be  the  skew- symmetric  angular  velocity 
matrix,  given  by 

n  =  (“  T)  <10> 

where  u  is  the  angular  velocity  of  the  pawl.  From  elementary  kinematics  (see,  for 
example,  [BR79]),  fi  =  . 

The  velocity  in  the  frame  attached  to  the  pawl,  RgTw,  can  now  be  calculated  by 
differentiation: 

ReTw  =  pr 

=  RsTr  +  R«tt 
=  R/r  +  RjCfr. 

Therefore,  (11) 

w  =  f  +  ClTr.  (12) 

The  differential  constraint  corresponds  to  the  departure  speed,  u,  being  zero.  There¬ 
fore, 

u  =  tom, 

=  r'„  -  u>rx 

=  0.  (13) 

Solving  for  w, 

rw 

u  =  •JL 


Hence,  the  sliding  speed,  s,  is  given  by 


3  =  wx 


=  ra  -I-  urw 
=  r.  +  ^r. 


=  —  Px  —  P» 

r4T 


The  map  of  sliding  speeds  for  Type- A  contact  is  almost  identical  to  Figure  4, 
except  that  the  direction  of  the  vector  p  is  reversed.  However,  the  orbit  of  the  hinge 
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point  during  sliding  is  more  complicated  than  in  the  type-B  case.  The  time  at  which 
the  hinge  point-  will  enter  Sector  III  and  stick  can  be  computed  as  follows.  At  the 
time  of  entry  into  Sector  III,  the  r  is  given  by 


r 


iri 

v/iTa? 

H 

y/T+li1 


(mi  -  nv) 


Hence, 

m’  “  |r| y/TTiZ  (  -m  l  )  r 
Plugging  this  back  in  Equation  1,  we  get 


(16) 


(17) 


r  m,  -  d, 


IrjvT+V 
lrl 


N /T+~P 


-d,= 


1 

0. 


1 


r  -  d, 


(18) 

(19) 


Since  r  is  a  linear  function  of  t,  this  is  a  quadratic  equation  in  time  and  can  be 
solved  for  t. 


4  Intersection  Problems  and  Collision  Detection 

As  the  linked  body  M  is  moved  in  the  environment  A/*,  the  pawl  Mn  may  collide  with 
am  obstacle.  We  need  robust  algorithms  to  detect  and  report  the  collision.  While 
a  pawl  is  sliding  on  a  feature  of  the  environment,  it  may  arrive  at  ^configuration  at 
which  it  is  kinematically  impossible  to  continue  to  maintain  contact  -  the  pawl  will 
either  snap  off  the  environment  or  jam  on  it.  We  need  a  way  to  determine  when  this 
occurs.  These  two  seemingly  unrelated  problems  are  in  fact  reducible  to  intersection 
problems  in  configuration  space. 

The  geometric  intersection  algorithms  for  our  problem,  however,  involve  several 
subtle  and  important  issues  to  make  them  robust  in  practice.  These  issues  have  a 
somewhat  different  flavor  than  the  rest  of  this  paper.  Hence  we  discuss  them  in 
detail  in  the  companion  paper  [DP89],  which  also  examines  a  host  of  important 
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practical  considerations  in  the  implementation  of  geometric  intersection  algorithms. 
We  shall  briefly  sketch  the  methods  used,  for  the  sake  of  completeness. 

Three  types  of  motion  are  possible  for  the  pawl  in  our  problem:  pure  transla¬ 
tion  without  rotation;  pure  rotation  without  translation;  and  most  importantly  the 
motion  of  the  pawl  sliding  while  maintaining  contact  with  a  given  feature  of  the 
obstacle.  The  collision  detection  problem  is  to  find  the  times  at  which  the  pawl 
collides  with  Af,  and  report  which  features  are  in  contact  during  collision. 

In  each  of  the  cases,  the  obstacle  constraint  equations  (1  and  2)  and  can  be 
reduced  to  algebraic  equations.  This  is  possible  since  the  trigonometric  functions 
in  the  rotation  matrix  R#  can  be  replaced  by  new  indeterminate  quantities  (eg. 
sin#  =  s,  cos#  =  c),  with  the  addition  of  a  new  constraint  (s2  4-  c2  —  1  =  0),  as 
in  [Don87|.  More  practically,  the  substitution  u  =  tan|  yields  constraint  equations 
that  are  quadratic  in  u,  with  coefficients  that  are  affine  in  i  and  y.  Since  r  and  y 
are  affinely  parametric  in  time  f,  the  coefficients  are  also  affine  in  f.  Intersecting 
two  of  these  constraints  requires  intersecting  two  quadratics.  Pure  rotational  inter¬ 
section  detection  (the  “snap”)  requires  solving  a  quadratic  in  u.  Pure  translational 
intersections  require  solving  an  affine  equation.  Hence  there  exists  a  closed-form, 
purely  algebraic  solution  to  these  intersection  problems.  The  collision  detection  for 
each  obstacle  constraint  can  be  done  in  constant  time,  since  the  degree,  size,  and 
number  of  variables  in  the  constraint  polynomials  is  fixed.  If  m  is  the  geometric 
complexity  of  the  pawl  and  n  is  that  of  the  size  of  the  environment,  there  are  mn 
such  constraints,  and  the  total  time  complexity  for  collision  detection  is  O (mn). 

Since  the  constraint  equations  are  quadratic  in  u,  it  possible  that  in  some  interval 
of  time  the  discriminant  A  of  the  constraint  equation  (treated  as  an  quadratic  in  u) 
becomes  negative,  yielding  no  solution  for  u.  This  corresponds  to  the  pawl  either 
having  snapped  off  the  constraint  or  having  jammed  on  it.  By  solving  for  the 
critical  times  t,  at  which  the  discriminant  changes  sign,  we  can  determine,  by  careful 
analysis,  which  case  occurs.  The  details  of  this  analysis  are  found  in  [DP89],  since 
it  is  related  to  the  problem  of  choosing  the  “correct  branch”  for  collision  detection. 

5  Resolving  Multiple  Contacts 

When  the  pawl  is  sliding  on  the  environment,  it  is  possible  for  a  second  contact  to 
occur  due  to  collision.  Generically,  the  pawl  can  maintain  two  contacts  for  only  an 
instant  in  time.  The  algorithm  has  to  determine  if  subsequent  motion  is  possible, 
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Figure  6:  Sticking  due  to  incompatible  constraints. 

environment  is  to  have  xii  =  0  (sliding  along  the  second  contact),  while  u\  >  0  (the 
first  contact  departing  from  its  edge). 

Finally,  to  ascertain  which  of  the  two  cases  occur,  we  only  need  to  check  the  sign 
of  the  ut -intercept  of  the  line.  From  equation  21,  this  is  given  by 

sgn  -  Pi2~)  •  (22) 

The  other  three  possible  transitions  are  similar.  As  before,  the  fact  that  de¬ 
parture  speeds  are  linked  by  being  on  the  same  pawl  leads  to  a  linear  relationship 
between  u*  and  ttj,  and  as  before,  the  pawl  will  stick  due  to  incompatible  kinematic 
contraints  or  the  second  contact  will  replace  the  first.  We  can  check  which  of  the 
two  occurs  by  checking  the  sign  of  the  u\  intercept  -  if  it  is  negative,  the  pawl  sticks. 
We  list  the  expressions  for  the  sign  of  the  ui  intercept  for  the  three  possible  cases 
below.  When  the  two  contacts  are  of  different  type,  we  shall  superscript  r  by  the 
contact  type  for  clarity,  since  the  interpretation  of  r  depends  on  the  contact  type. 

If  both  contacts  are  Type- A: 

sgn  (r'vi  -  .  (23) 

If  the  first  contact  is  Type-B  and  the  second  contact  is  Type- A: 

sgn  •  (24) 

If  the  first  contact  is  Type-A  and  the  second  contact  is  Type-B: 

sgn  ^  +Pv3p^  •  (25) 
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6  The  Algorithm 

Putting  all  the  pieces  developed  in  the  previous  sections  together,  we  present  the 
following  algorithm  for  the  motion  of  a  single  pawl.  In  order  to  keep  the  number 
of  special  cases  small,  we  describe  the  algorithm  for  type-B  contacts  only.  It  is 
straightforward  to  handle  type- A  contacts  as  well. 

Preprocessing:  Determine  the  possible  collisions  of  the  pawl  with  M  while  under¬ 
going  a  pure  translation  of  p.  Let  the  first  such  time  be  t*,.  Also  determine 
the  time  te  at  which  the  root  polygon  Ado  collides  with  A/\  The  algorithm  can 
now  be  concerned  only  with  the  time  interval 

Step  1:  For  the  current  contact,  determine  the  possible  direction  of  slide,  using  the 
results  of  Section  3. 

Step  2:  Determine  if  sticking  occurs  due  to  friction,  using  the  results  of  Section  2. 
If  it  sticks,  report  this,  and  END. 

Else,  determine  the  time  t ,  at  which  the  contact  enters  the  sticking  region, 
using  the  results  of  Section  3. 

Step  3:  Determine  the  time  at  which  the  pawl  jams  on  the  constraint  due  to  the 
kinematics  or  snaps  off,  using  the  results  of  [DP89]. 

If  it  jams,  report  this,  and  END. 

Else,  the  pawl  has  snapped  off  the  constraint.  The  pawl  now  undergoes  a  pure 
rotation  towards  its  rest  position.  Determine  the  next  collision  during  this 
rotation. 

•  If  a  new  contact  is  found,  report  it.  GOTO  Step  1  and  repeat  with  the 
new  contact. 

•  Else  find  the  next  collision  under  pure  translation  after  the  rotation  has 
finished.  Report  the  collisioaJound.  GOTO  Step  1  and  repeat  with  the 
new  contact.  If  no  collision  is  found,  END. 
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Step  4:  Determine  if  other  collisions  between  the  pawl  and  Af  occur  while  the  pawl 
is  sliding- while  maintaining  the  current  contact,  using  the  results  of  Section  4. 
Let  te  be  the  time  of  the  first  such  collision.  Note  that  one  also  obtains  the 
time  at  which  the  contact  reaches  the  end  of  the  current  edge  t},  i.e.,  the  time 
at  which  a  collision  occurs  between  the  contact  vertex  and  the  adjacent  edge. 
Let  this  time  be  tf. 

If  min{fe,  t, }  =  t,  then  report  that  the  pawl  has  stuck  due  to  friction,  END; 

Else,  if  tc  as  tf,  the  contact  vertex  has  reached  the  end  of  the  current  edge. 
The  pawl  now  snaps  back  toward  its  rest  position.  Find  the  next  collision  as 
in  Step  3.  Note  that  the  transition  to  an  adjacent  edge  is  included  in  this  test. 

Else,  if  min{te,  t,}  =  tc,  report  the  new  contact  that  has  occurred.  Analyze 
the  multiple  contacts  using  the  results  of  Section  5. 

•  If  the  new  contact  is  incompatible,  report  this,  and  END;  the  pawl  has 
stuck  due  to  incompatible  kinematic  constraints. 

•  Else,  the  new  contact  is  effective.  GOTO  Step  1  and  repeat  with  the  new 
contact. 


End 

Each  collision  detection  step  above  takes  time  O(mn)  [Don87].  In  addition, 
suppose  the  contact  changes  c  times.  Then  the  loop  of  Steps  1-4  is  repeated  c 
times.  Therefore  the  algorithm  takes  time  O(cmn).  We  expect  (but  do  not  prove) 
that  c  =  O(mn),  and  that  it  will  typically  be  much  smaller.  However,  in  order  to 
obtain  an  upper  bound,  we  offer  the  following  modification  to  the  above  algorithm, 
yielding  a  running  time  of  0(m2n2  log  mn). 

We  h*.ndl«»  the  collision  detection  while  sliding  via  the  following  data  structure: 
with  each  constraint  equation,  we  associate  two  balanced  search  trees,  labeled  Plus 
and  Minus.  The  Plus  tree  corresponds  to  contact  with  the  constraint  with  positive 
Branch-Sign,  while  the  Minus  tree  corresponds  to  contact  with  negative  Branch- 
Sign.  See  [DP89].  Essentially,  the  Branch-Sign  is  used  to  choose  the  correct  root  in 
the  solving  the  intersection  problem  for  collision  detection.  For  type-B  constraints, 
for  example,  the  Branch-Sign  is  the  sign  of  r  x  n;.  The  Branch-Sign  is  invariant 
during  the  motion  along  the  constraint.  The  leaves  of  each  tree  contain,  sorted 
by  the  time  t,  all  the  tuples  (t,  k)  where  k  =  1, . . . ,  mn,  is  the  index  of  a  contact 
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constraint  equation,  and  t  is  the  time  at  which  the  constraint  is  satisfied  by  p(t). 
Generically,  the  number  of  different  times  t  for  a  given  k  has  a  fixed  upper  bound 
which  can  be  obtained  from  Bezout’s  Theorem  [vdW50j.  Hence,  there  are  0(mn) 
nodes  in  each  tree,  and  hence  each  tree  takes  0(mn  log  mn)  to  build,  and  hence  the 
entire  data  structure  takes  0(m2n2  log  mn)  time  to  build.  Each  collision  detection 
step  of  the  algorithm  now  takes  0(log  mn). 

We  claim  that  the  collision  detection  can  be  requested  at  most  as  many  times 
as  there  axe  leaves  in  the  data  structure.  Suppose  we  request  the  next  collision  at 
time  t,  while  maintaining  constraint  ki  with  a  given  Branch-Sign,  and  the  answer  is 
the  tuple  (te,  k2).  From  the  algorithm  it  follows  that  only  four  things  can  happen: 

1.  The  pawl  sticks  due  to  friction,  or  due  to  kinematic  constraints,  in  the  time 
interval  [t,  ie],  and  the  program  terminates. 

2.  The  pawl  snaps  off  the  end  of  an  edge  at  time  tc  —  tf.  This  implies  that  leaf 
of  the  tree  with  time  tj  and  constraint  corresponding  to  the  contact  vertex 
touching  an  adjacent  edge  are  used  up,  and  the  time  is  advanced  to  tf. 

3.  The  pawl  snaps  off  the  interior  of  edge,  since  the  contact  can  no  longer  be 
maintained  with  the  constraint  k\.  Hence  both  the  subtrees  under  kx  are  used 
up,  since  the  pawl  can  never  return  to  kx. 

4.  (te,  k2)  is  the  new  contact.  Hence  this  leaf  is  used  up  and  the  time  is  advanced 
to  te. 

Hence,  no  tuple  can  be  the  answer  to  a  collision  detection  request  more  than  once, 
proving  our  claim.  Since  the  entire  data  structure  has  0(m3n3)  leaves,  the  collision 
detection  while  sliding  takes  0(m2n2  log  mn)  time.  We  can  similarly  show  that  the 
collision  detection  problem  for  pure  translation  and  pure  rotation  can  be  done  in 
0(m2n3  log  mn)  . 

7  Conclusions 

In  this  paper,  we  addressed  the  problem  of  predicting  the  motion  of  a  flexible  object 
a.midat  a  polygonal  environment.  To  this  end,  we  presented  a  careful  analysis  of 
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the  physics  of  the  interaction  of  the  flexible  body  with  the  environment.  The  anal¬ 
ysis  yielded  several  simple  tests  to  determine  the  motion  of  the  body,  which  were 
integrated  into  an  algorithm  for  predicting  the  motion  under  a  given  motion  plan 

This  motion  prediction  problem  is  the  first  step  in  reasoning  about  such  devices. 
The  companion  paper  [DP 89]  describes  the  implementation  of  the  algorithm  and 
related  issues,  as  well  the  significance  of  the  algorithm  for  designing  compliant  parts 
for  ease  of  assembly  and  the  effect  of  uncertainty.  Future  work  includes  the  incorpo¬ 
ration  of  more  complicated  models  of  the  flexible  objects  (such  as  that  of  [TM87]) 
and  the  dynamics  of  interaction. 
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